#ifndef __MQ3D_MATH_H__
#define __MQ3D_MATH_H__


static float PI = 3.14159265f;

static inline 
float dot (const MQPoint& a, const MQPoint& b)
{
	return a.x*b.x + a.y*b.y + a.z*b.z;
}

static inline 
MQPoint cross (const MQPoint& a, const MQPoint& b)
{
	return MQPoint(a.y*b.z-a.z*b.y, a.z*b.x-a.x*b.z, a.x*b.y-a.y*b.x);
}

static inline
void invert (float* m) {
	float n[16];
    memcpy (n, m, sizeof(float)*16);

    float det = 
        + n[0]*n[5]*n[10]*n[15] + n[0]*n[6]*n[11]*n[13] + n[0]*n[7]*n[9 ]*n[14]
        + n[1]*n[4]*n[11]*n[14] + n[1]*n[6]*n[8 ]*n[15] + n[1]*n[7]*n[10]*n[12]
        + n[2]*n[4]*n[9 ]*n[15] + n[2]*n[5]*n[11]*n[12] + n[2]*n[7]*n[8 ]*n[13]
        + n[3]*n[4]*n[10]*n[13] + n[3]*n[5]*n[8 ]*n[14] + n[3]*n[6]*n[9 ]*n[12]
        - n[0]*n[5]*n[11]*n[14] - n[0]*n[6]*n[9 ]*n[15] - n[0]*n[7]*n[10]*n[13]
        - n[1]*n[4]*n[10]*n[15] - n[1]*n[6]*n[11]*n[12] - n[1]*n[7]*n[8 ]*n[14]
        - n[2]*n[4]*n[11]*n[13] - n[2]*n[5]*n[8 ]*n[15] - n[2]*n[7]*n[9 ]*n[12]
        - n[3]*n[4]*n[9 ]*n[14] - n[3]*n[5]*n[10]*n[12] - n[3]*n[6]*n[8 ]*n[13];

    float inv_det = 1/det;

    m[0]  = inv_det*(n[5]*n[10]*n[15] + n[6]*n[11]*n[13] + n[7]*n[9 ]*n[14] - n[5]*n[11]*n[14] - n[6]*n[9 ]*n[15] - n[7]*n[10]*n[13]);
    m[1]  = inv_det*(n[1]*n[11]*n[14] + n[2]*n[9 ]*n[15] + n[3]*n[10]*n[13] - n[1]*n[10]*n[15] - n[2]*n[11]*n[13] - n[3]*n[9 ]*n[14]);
    m[2]  = inv_det*(n[1]*n[6 ]*n[15] + n[2]*n[7 ]*n[13] + n[3]*n[5 ]*n[14] - n[1]*n[7 ]*n[14] - n[2]*n[5 ]*n[15] - n[3]*n[6 ]*n[13]);
    m[3]  = inv_det*(n[1]*n[7 ]*n[10] + n[2]*n[5 ]*n[11] + n[3]*n[6 ]*n[9 ] - n[1]*n[6 ]*n[11] - n[2]*n[7 ]*n[9 ] - n[3]*n[5 ]*n[10]);
    m[4]  = inv_det*(n[4]*n[11]*n[14] + n[6]*n[8 ]*n[15] + n[7]*n[10]*n[12] - n[4]*n[10]*n[15] - n[6]*n[11]*n[12] - n[7]*n[8 ]*n[14]);
    m[5]  = inv_det*(n[0]*n[10]*n[15] + n[2]*n[11]*n[12] + n[3]*n[8 ]*n[14] - n[0]*n[11]*n[14] - n[2]*n[8 ]*n[15] - n[3]*n[10]*n[12]);
    m[6]  = inv_det*(n[0]*n[7 ]*n[14] + n[2]*n[4 ]*n[15] + n[3]*n[6 ]*n[12] - n[0]*n[6 ]*n[15] - n[2]*n[7 ]*n[12] - n[3]*n[4 ]*n[14]);
    m[7]  = inv_det*(n[0]*n[6 ]*n[11] + n[2]*n[7 ]*n[8 ] + n[3]*n[4 ]*n[10] - n[0]*n[7 ]*n[10] - n[2]*n[4 ]*n[11] - n[3]*n[6 ]*n[8 ]);
    m[8]  = inv_det*(n[4]*n[9 ]*n[15] + n[5]*n[11]*n[12] + n[7]*n[8 ]*n[13] - n[4]*n[11]*n[13] - n[5]*n[8 ]*n[15] - n[7]*n[9 ]*n[12]);
    m[9]  = inv_det*(n[0]*n[11]*n[13] + n[1]*n[8 ]*n[15] + n[3]*n[9 ]*n[12] - n[0]*n[9 ]*n[15] - n[1]*n[11]*n[12] - n[3]*n[8 ]*n[13]);
    m[10] = inv_det*(n[0]*n[5 ]*n[15] + n[1]*n[7 ]*n[12] + n[3]*n[4 ]*n[13] - n[0]*n[7 ]*n[13] - n[1]*n[4 ]*n[15] - n[3]*n[5 ]*n[12]);
    m[11] = inv_det*(n[0]*n[7 ]*n[9 ] + n[1]*n[4 ]*n[11] + n[3]*n[5 ]*n[8 ] - n[0]*n[5 ]*n[11] - n[1]*n[7 ]*n[8 ] - n[3]*n[4 ]*n[9 ]);
    m[12] = inv_det*(n[4]*n[10]*n[13] + n[5]*n[8 ]*n[14] + n[6]*n[9 ]*n[12] - n[4]*n[9 ]*n[14] - n[5]*n[10]*n[12] - n[6]*n[8 ]*n[13]);
    m[13] = inv_det*(n[0]*n[9 ]*n[14] + n[1]*n[10]*n[12] + n[2]*n[8 ]*n[13] - n[0]*n[10]*n[13] - n[1]*n[8 ]*n[14] - n[2]*n[9 ]*n[12]);
    m[14] = inv_det*(n[0]*n[6 ]*n[13] + n[1]*n[4 ]*n[14] + n[2]*n[5 ]*n[12] - n[0]*n[5 ]*n[14] - n[1]*n[6 ]*n[12] - n[2]*n[4 ]*n[13]);
    m[15] = inv_det*(n[0]*n[5 ]*n[10] + n[1]*n[6 ]*n[8 ] + n[2]*n[4 ]*n[9 ] - n[0]*n[6 ]*n[9 ] - n[1]*n[4 ]*n[10] - n[2]*n[5 ]*n[8 ]);
}

static inline
void transpose (float* m)
{
	float n[16];
    memcpy (n, m, sizeof(float)*16);

    m[0] = n[0];
    m[1] = n[4];
    m[2] = n[8];
    m[3] = n[12];

    m[4] = n[1];
    m[5] = n[5];
    m[6] = n[9];
    m[7] = n[13];

    m[8]  = n[2];
    m[9]  = n[6];
    m[10] = n[10];
    m[11] = n[14];

    m[12] = n[3];
    m[13] = n[7];
    m[14] = n[11];
    m[15] = n[15];
}



#endif
